#ifndef __HEAP_POLICY__
#define __HEAP_POLICY__

#include "heap_policy.hpp" // declares heap_policy
#include <cmath> // defines ilogb
#include <iostream> // defines std::cerr

extern int ilogb  (double) throw();

template <> template <typename position, typename ordering>
heap_policy<2, position, ordering>::heap_policy (
  heap_policy<2, position, ordering>::index n
): n(n) {
}

template <> template <typename position, typename ordering>
typename heap_policy<2, position, ordering>::index
heap_policy<2, position, ordering>::size () const {
  return n;
}

template <> template <typename position, typename ordering>
typename heap_policy<2, position, ordering>::index
    heap_policy<2, position, ordering>::last_leaf () const {
  return n - 1;
}

template <> template <typename position, typename ordering>
typename heap_policy<2, position, ordering>::index
heap_policy<2, position, ordering>::last_child (
  heap_policy<2, position, ordering>::index i
) const { 
  if(i & 4) return (i << 4) - ((i >> 3) << 6) - 48;
  return ((i & -8) | ((i & 3) << 1) + 1);
}

template <> template <typename position, typename ordering>
typename heap_policy<2, position, ordering>::index
heap_policy<2, position, ordering>::first_child (
  heap_policy<2, position, ordering>::index i
) const {
  if(i & 4) return (i << 4) - ((i >> 3) << 6) - 56;
  if((i & 7) == 0) 
    return i | 1;
  return ((i & -8) | ((i & 3) << 1));
}

template <> template <typename position, typename ordering>
typename heap_policy<2, position, ordering>::index
heap_policy<2, position, ordering>::parent (
  heap_policy<2, position, ordering>::index i
) const {
  if((i&7)== 0) {
    index j = (i >> 3) - 1; 
    index t = (j >> 3) << 3;
    j = ((j & 7) >> 1) + 4;
    return (j + t);
  }
  else {
    index j = (i >> 3) << 3;;
    j = j + ((i&7) >> 1);
    return j;
  }
}

template <> template <typename position, typename ordering>
typename heap_policy<2, position, ordering>::index
heap_policy<2, position, ordering>::top_some_absent (
  position a,
  heap_policy<2, position, ordering>::index i,
  const ordering& less
) const {
  return first_child(i);  
}

template <> template <typename position, typename ordering>
typename heap_policy<2, position, ordering>::index
heap_policy<2, position, ordering>::top_all_present (
  position a,
  heap_policy<2, position, ordering>::index i,
  const ordering& less
) const {
  typedef typename heap_policy<2, position, ordering>::index index;
  index k = i & 7;
  if(k == 0) return (i | 1); 
  index j;
  if((i & 4) == 0) { 
    k = (i & -8) | (k << 1);
    j = k + 1;
    return less(*(a + j), *(a + k)) ? k: j;
  }
  k = (i << 4) - ((i >> 3) << 6) - 48;
  j = k - 8;
  return less(*(a + k), *(a + j)) ? j : k;
}

template <> template <typename position, typename ordering>
void
heap_policy<2, position, ordering>::update_some_absent (
  position a,
  heap_policy<2, position, ordering>::index i,
  const heap_policy<2, position, ordering>::element& x,
  const ordering& less
) {
  a[i] = x;
}

template <> template <typename position, typename ordering>
void
heap_policy<2, position, ordering>::update_all_present (
  position a,
  heap_policy<2, position, ordering>::index i,
  const heap_policy<2, position, ordering>::element& x,
  const ordering&
) {
  a[i] = x;
}

template <> template <typename position, typename ordering>
void
heap_policy<2, position, ordering>::visit_branch_first_time (
  position,
  heap_policy<2, position, ordering>::index,
  const ordering&
) {
  // Does nothing
}

template <> template <typename position, typename ordering>
void
heap_policy<2, position, ordering>::erase_last_leaf (position) {
  --n;
}

#endif // __HEAP_POLICY__

